# Copyright (c) 2022-2024, The Berkeley Humanoid Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

import isaaclab.sim as sim_utils
from isaaclab.actuators import DelayedPDActuatorCfg
from isaaclab.assets.articulation import ArticulationCfg

from skyentific_poclegs.assets import ISAAC_ASSET_DIR


SKYENTIFIC_POCLEGS_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path=f"{ISAAC_ASSET_DIR}/robots/skyentific_poclegs/poclegs.usd",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            retain_accelerations=False,
            linear_damping=0.0,
            angular_damping=0.0,
            max_linear_velocity=1000.0,
            max_angular_velocity=1000.0,
            max_depenetration_velocity=1.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.449),
        joint_pos={
            'LL_HR': 0.0,
            'LR_HR': 0.0,
            'LL_HAA': -0.1745,
            'LR_HAA': -0.1745,
            'LL_HFE': -0.1745,
            'LR_HFE': -0.1745,
            'LL_KFE': 0.3491,
            'LR_KFE': 0.3491,
            'LL_FFE': -0.1745,
            'LR_FFE': -0.1745
        },
    ),
    actuators={
        "hr": DelayedPDActuatorCfg(
            joint_names_expr=[".*HR"],
            effort_limit=24.0,
            velocity_limit=23.0,
            stiffness=10.0,
            damping=1.5,
            armature=6.9e-5 * 81,
            friction=0.02,
            min_delay=0,  # physics time steps (min: 2.0*0=0.0ms)
            max_delay=4,  # physics time steps (max: 2.0*4=8.0ms)
        ),
        "haa": DelayedPDActuatorCfg(
            joint_names_expr=[".*HAA"],
            effort_limit=30.0,
            velocity_limit=15.0,
            stiffness=15.0,
            damping=1.5,
            armature=9.4e-5 * 81,
            friction=0.02,
            min_delay=0,  # physics time steps (min: 2.0*0=0.0ms)
            max_delay=4,  # physics time steps (max: 2.0*4=8.0ms)
        ),
        "kfe": DelayedPDActuatorCfg(
            joint_names_expr=[".*HFE", ".*KFE"],
            effort_limit=30.0,
            velocity_limit=20.0,
            stiffness=15.0,
            damping=1.5,
            armature=1.5e-4 * 81,
            friction=0.02,
            min_delay=0,  # physics time steps (min: 2.0*0=0.0ms)
            max_delay=4,  # physics time steps (max: 2.0*4=8.0ms)
        ),
        "ffe": DelayedPDActuatorCfg(
            joint_names_expr=[".*FFE"],
            effort_limit=20.0,
            velocity_limit=23.0,
            stiffness=10.0,
            damping=1.5,
            armature=6.9e-5 * 81,
            friction=0.02,
            min_delay=0,  # physics time steps (min: 2.0*0=0.0ms)
            max_delay=4,  # physics time steps (max: 2.0*4=8.0ms)
        ),
    },
    soft_joint_pos_limit_factor=0.95,
)

# 定义DeepRobotics Duck机器人的关节配置
DEEPROBOTICS_DUCK_CFG = ArticulationCfg(
    # 机器人生成相关的URDF文件配置
    spawn=sim_utils.UrdfFileCfg(
        fix_base=False, # 是否固定基座
        asset_path=f"{ISAAC_ASSET_DIR}/robots/duck_description/urdf/duck_10dofs.urdf", # 机器人URDF文件路径
        activate_contact_sensors=True, # 是否激活接触传感器
        # 刚体属性配置
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=False, # 是否禁用重力
                retain_accelerations=False, # 是否保留加速度
                linear_damping=0.0, # 线性阻尼
                angular_damping=0.0, # 角阻尼
                max_linear_velocity=1000.0, # 最大线速度
                max_angular_velocity=1000.0, # 最大角速度
                max_depenetration_velocity=1.0, # 最大去穿透速度
        ),
        # 关节根属性配置
        # 如果想要仿真的更准确，可以增加solver_position_iteration_count和solver_velocity_iteration_count的值，
        # 数值越大，仿真计算越精确，但计算开销也会增加。
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=False,     # 启用自碰撞
                solver_position_iteration_count=8, # 位置迭代次数
                solver_velocity_iteration_count=4  # 速度迭代次数
        ),
        # 关节驱动配置
        joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
                gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0) # PD增益配置
        ),
    ),
    # 初始状态配置
    init_state=ArticulationCfg.InitialStateCfg(
        # 初始位置
        pos=(0.0, 0.0, 0.3),
        # 关节初始位置
        joint_pos={
                "left_hip_yaw_joint": 0.0,
                "left_hip_roll_joint": 0.0,
                "left_hip_pitch_joint": 0.7,
                "left_knee_joint": -0.83,
                "left_ankle_joint": 0.2,
                "right_hip_yaw_joint": 0.0,
                "right_hip_roll_joint": 0.0,
                "right_hip_pitch_joint": 0.7,
                "right_knee_joint": -0.83,
                "right_ankle_joint": 0.2
        },
        # 所有关节初始速度为0
        joint_vel={".*": 0.0},
    ),
    soft_joint_pos_limit_factor=0.9, # 软关节位置限制因子
    # 执行器配置
    actuators={
        "motor": DelayedPDActuatorCfg(
            joint_names_expr=[
                ".*_hip_yaw_joint",     # left_hip_yaw_joint, right_hip_yaw_joint
                ".*_hip_roll_joint",    # left_hip_roll_joint, right_hip_roll_joint
                ".*_hip_pitch_joint",   # left_hip_pitch_joint, right_hip_pitch_joint
                ".*_knee_joint",        # left_knee_joint, right_knee_joint
                ".*_ankle_joint",       # left_ankle_joint, right_ankle_joint
            ],
            stiffness = {
                ".*_hip_yaw_joint":   25.0,
                ".*_hip_roll_joint":  40.0,
                ".*_hip_pitch_joint": 40.0,
                ".*_knee_joint":      40.0,
                ".*_ankle_joint":     25.0,
            },
            damping = {
                ".*_hip_yaw_joint":   0.5,
                ".*_hip_roll_joint":  1.0,
                ".*_hip_pitch_joint": 1.0,
                ".*_knee_joint":      1.0,
                ".*_ankle_joint":     0.5,
            },
            effort_limit = {
                ".*_hip_yaw_joint":   23.7,
                ".*_hip_roll_joint":  33.5,
                ".*_hip_pitch_joint": 33.5,
                ".*_knee_joint":      33.5,
                ".*_ankle_joint":     23.7,
            },
            velocity_limit = {
                ".*_hip_yaw_joint":   30,
                ".*_hip_roll_joint":  21,
                ".*_hip_pitch_joint": 21,
                ".*_knee_joint":      21,
                ".*_ankle_joint":     30,
            },
            friction=0.02,        # 摩擦
            armature=6.9e-5 * 81, # 转动惯量
            min_delay=0,          # 最小延迟
            max_delay=5,          # 最大延迟，机器人延迟通常在0~25ms之间；仿真物理步长是5ms，max_delay定义了有多少物理步长会被延迟
        ),
    },
)
